hud_setting = {
    width = 512,
    height = 256,
    scale = 2,
    head_distance = 3,

    angle_per_scale = 10,
    roll_line_radius = 45,
    roll_line_radius2 = 65,
    hud_frame_delay = 0.02,

    -- generated parameter
    inner_scale = 16 / 256,
    centerX = 0, --(hud_setting.width-1) / 2,
    centerY = 0, --(hud_setting.height-1) / 2
    half_range = 10,
    _proj_len_var = 128
}

hud_color = {
    bg_color = 0x40FF4000,
    roll_line = 0xFFFFFFFF,
    roll_line_zero = 0x40FF40FF,
    roll_line_sky = 0xFF4040FF,
    roll_line_ground = 0x4040FFFF,
    pitch_line = 0x60FF60FF,
    info_text = 0x20FF20FF
}

--local mon = peripheral.find("monitor")

local vector = dofile("vector.lua")
local input = dofile("input.lua")
local hub = dofile("hub.lua")

function FindPeripheral(p)
    return peripheral.find(p) or hub.wrap(hub.getPeripherals(p)[1])[1]
end
local holo = FindPeripheral("hologram")
local ship = FindPeripheral("EngineController")
local ctrl = FindPeripheral("tweaked_controller")

input_channel = input:NewChannel()
input_channel:BindController(ctrl)

ship_pose = {
    pitch = 0,
    yaw = 0,
    roll = 0,
    horizontal_angle = 0
}


function h(x)
    return math.floor(x+0.5)
end

vec2 = {}
function vec2.mul(x1,y1,x2,y2)
    return x1*x2 , y1*y2
end

function vec2.scale(x1, y1, s)
    return x1*s, y1*s
end

function vec2.add(x1,y1,x2,y2)
    return x1+x2, y1+y2
end

function vec2.fma(s, x1, y1, x2, y2)
    return x1*s+x2, y1*s+y2
end

function h_(x)
    return tostring(math.floor(x * 100 + 0.5) / 100)
end

function InitHologram()
    hud_setting.centerX = (hud_setting.width-1) / 2
    hud_setting.centerY = (hud_setting.height-1) / 2

    hud_setting.inner_scale = 16 / hud_setting.height
    local s = hud_setting.inner_scale * hud_setting.scale
    hud_setting.half_range = math.atan(
            (hud_setting.scale * (hud_setting.width / hud_setting.height) / 2)
                    / hud_setting.head_distance)
    hud_setting._proj_len_var = hud_setting.head_distance * hud_setting.height / 2

    holo.SetCurrentFrameBuffer(-1)
    holo.Resize(hud_setting.width,hud_setting.height)
    holo.SetRotation(0, 0, 0)
    holo.SetTranslation(0, 1, 0)
    holo.SetScale(s,s)
    holo.SetClearColor(hud_color.bg_color)
    holo.Clear()
    holo.Flush()
end

function HUD_Thread()
    os.sleep(0.1)

    local h_x, h_y, v_x, v_y = 0,0
    local draw_angle, draw_angle_rad, delta_angle, v_m = 0, 0, 0, 0
    local scaled_vx, scaled_vy = 0,0
    local scaled_hx, scaled_hy = 0,0
    local scaled_hx2, scaled_hy2 = 0,0
    local text_x, text_y = 0,0
    local range_min, range_max = 0,0
    local tmp_x, tmp_y = 0,0
    local txt = ""
    local velocity_text = 0

    while true do
        holo.Clear()

        -- Roll
        h_x = math.cos(-ship_pose.roll)
        h_y = math.sin(-ship_pose.roll)
        v_x = -h_y
        v_y = h_x

        draw_angle, draw_angle_rad, delta_angle, v_m = 0, 0, 0, 0
        scaled_vx, scaled_vy = 0,0
        scaled_hx, scaled_hy = h_x * hud_setting.roll_line_radius, h_y * hud_setting.roll_line_radius
        scaled_hx2, scaled_hy2 = h_x * hud_setting.roll_line_radius2, h_y * hud_setting.roll_line_radius2
        text_x, text_y = h_x * 8, h_y * 8
        tmp_x, tmp_y = 0,0

        range_min, range_max = math.deg(ship_pose.pitch - hud_setting.half_range),  math.deg(ship_pose.pitch + hud_setting.half_range)

        while true do
            if draw_angle >= 179 then break end
            draw_angle_rad = math.rad(draw_angle)

            if draw_angle > 90 then
                txt = tostring(180 - draw_angle)
            else
                txt = tostring(draw_angle)
            end

            if draw_angle >= range_min and draw_angle <= range_max then
                delta_angle = draw_angle_rad - ship_pose.pitch
                v_m = math.tan(delta_angle) * hud_setting._proj_len_var

                scaled_vx, scaled_vy = vec2.scale(v_x, v_y, v_m)

                tmp_x, tmp_y  = hud_setting.centerX + scaled_vx + scaled_hx2, hud_setting.centerY - scaled_vy - scaled_hy2
                if draw_angle == 0 then
                    holo.DrawLine(
                            h(hud_setting.centerX + scaled_vx - scaled_hx), h(hud_setting.centerY - scaled_vy + scaled_hy),
                            h(hud_setting.centerX + scaled_vx - scaled_hx2), h(hud_setting.centerY - scaled_vy + scaled_hy2),
                            hud_color.roll_line_zero
                    )
                    holo.DrawLine(
                            h(hud_setting.centerX + scaled_vx + scaled_hx), h(hud_setting.centerY - scaled_vy - scaled_hy),
                            h(tmp_x), h(tmp_y),
                            hud_color.roll_line_zero
                    )

                    holo.Text(
                            h(tmp_x + text_x - 4), h(tmp_y - text_y - 8),
                            txt, hud_color.roll_line_zero, 0
                    )
                else
                    holo.DrawLine(
                            h(hud_setting.centerX + scaled_vx - scaled_hx), h(hud_setting.centerY - scaled_vy + scaled_hy),
                            h(hud_setting.centerX + scaled_vx - scaled_hx2), h(hud_setting.centerY - scaled_vy + scaled_hy2),
                            hud_color.roll_line_sky
                    )
                    holo.DrawLine(
                            h(hud_setting.centerX + scaled_vx + scaled_hx), h(hud_setting.centerY - scaled_vy - scaled_hy),
                            h(tmp_x), h(tmp_y),
                            hud_color.roll_line_sky
                    )

                    holo.Text(
                            h(tmp_x + text_x - 8), h(tmp_y - text_y - 8),
                            txt, hud_color.roll_line_zero, 0
                    )
                end

            end

            if draw_angle > 0 and -draw_angle >= range_min and -draw_angle <= range_max then
                delta_angle = -draw_angle_rad - ship_pose.pitch
                v_m = math.tan(delta_angle) * hud_setting._proj_len_var

                scaled_vx, scaled_vy = vec2.scale(v_x, v_y, v_m)

                tmp_x, tmp_y  = hud_setting.centerX + scaled_vx + scaled_hx2, hud_setting.centerY - scaled_vy - scaled_hy2
                holo.DrawLine(
                        h(hud_setting.centerX + scaled_vx - scaled_hx), h(hud_setting.centerY - scaled_vy + scaled_hy),
                        h(hud_setting.centerX + scaled_vx - scaled_hx2), h(hud_setting.centerY - scaled_vy + scaled_hy2),
                        hud_color.roll_line_ground
                )
                holo.DrawLine(
                        h(hud_setting.centerX + scaled_vx + scaled_hx), h(hud_setting.centerY - scaled_vy - scaled_hy),
                        h(tmp_x), h(tmp_y),
                        hud_color.roll_line_ground
                )

                holo.Text(
                        h(tmp_x + text_x - 8), h(tmp_y - text_y - 8),
                        txt, hud_color.roll_line_zero, 0
                )
            end
            draw_angle = draw_angle + hud_setting.angle_per_scale
        end

        scaled_hx, scaled_hy = h_x * (hud_setting.roll_line_radius2 + 20), h_y * (hud_setting.roll_line_radius2 + 20)
        local p_x, p_y = h_x * (hud_setting.roll_line_radius2 + 75), h_y * (hud_setting.roll_line_radius2 + 75)

        holo.DrawLine(
                h(hud_setting.centerX + scaled_hx), h(hud_setting.centerY - scaled_hy),
                h(hud_setting.centerX + p_x), h(hud_setting.centerY - p_y),
                hud_color.roll_line
        )

        holo.DrawLine(
                h(hud_setting.centerX - scaled_hx), h(hud_setting.centerY + scaled_hy),
                h(hud_setting.centerX - p_x), h(hud_setting.centerY + p_y),
                hud_color.roll_line
        )

        local r = 12
        scaled_hx, scaled_hy = h_x * r, h_y * r
        p_x, p_y = v_x * r, v_y * r
        
        holo.DrawLine(
                h(hud_setting.centerX - scaled_hx), h(hud_setting.centerY + scaled_hy),
                h(hud_setting.centerX + scaled_hx), h(hud_setting.centerY - scaled_hy),
                hud_color.roll_line
        )

        holo.DrawLine(
                h(hud_setting.centerX - p_x), h(hud_setting.centerY + p_y),
                h(hud_setting.centerX + p_x), h(hud_setting.centerY - p_y),
                hud_color.roll_line
        )

        -- Draw Info
        if velocity_len > 300 then
            velocity_text = h_(velocity_len / 340).."Ma"
        else
            velocity_text = h_(velocity_len).." m/s"
        end

        holo.Text(10, 10, "\\u901f\\u5ea6 "..velocity_text, hud_color.info_text, 0)
        holo.Text(10, 30, "\\u6d77\\u62d4 "..h_(pose.pos.y - 60).." m", hud_color.info_text, 0)

        -- Draw Edge
        holo.DrawLine(0, 0, 0, 50, 0xFFFFFFFF)
        holo.DrawLine(0, 0, 50, 0, 0xFFFFFFFF)

        holo.DrawLine(hud_setting.width-1, 0, hud_setting.width-51, 0, 0xFFFFFFFF)
        holo.DrawLine(hud_setting.width-1, 0, hud_setting.width-1, 50, 0xFFFFFFFF)

        holo.DrawLine(0, hud_setting.height-1, 0, hud_setting.height-51, 0xFFFFFFFF)
        holo.DrawLine(0, hud_setting.height-1, 50, hud_setting.height-1, 0xFFFFFFFF)

        holo.DrawLine(hud_setting.width-1, hud_setting.height-1, hud_setting.width-1, hud_setting.height-51, 0xFFFFFFFF)
        holo.DrawLine(hud_setting.width-1, hud_setting.height-1, hud_setting.width-51, hud_setting.height-1, 0xFFFFFFFF)

        holo.Flush()

        os.sleep(hud_setting.hud_frame_delay)
    end
end


function OnPhysUpdate(phys)
    input_channel:Update()
    faces = phys.getControllerFacesVec()
    matrix = phys.getControllerRotationMat()
    inerita = phys.getInertia()
    pose = phys.getShipPoseVel()
    euler = phys.getControllerEuler()

    ship_pose.yaw, ship_pose.pitch, ship_pose.roll = euler.yaw, euler.pitch, euler.roll

    c_yaw = -input_channel:GetAxis(1);
    c_fwd = input_channel:GetAxis(5) - input_channel:GetAxis(6);

    c_up = -input_channel:GetAxis(2)

    c_roll = -input_channel:GetAxis(3);
    c_pitch = -input_channel:GetAxis(4);

    c_brake = input_channel:GetButton(1);

    velocity = pose.velocity
    velocity_len = vector.Length(velocity)
    _omega = vector.Negative(pose.omega)
    tensor = inerita.momentOfInertiaTensor[1][1]
    tmp = {}

    -- config
    rot_damping = 15
    rot_acc = 40

    brake_force = -inerita.mass

    move_acc = 100
    if c_brake then
        brake_force = brake_force*7
    end

    -- hover
    ship.applyInvariantForce(0, 10 * inerita.mass, 0)

    -- air brake
    --   move damping
    tmp = vector.MulNum(velocity, brake_force)
    ship.applyInvariantForce(tmp.x, tmp.y, tmp.z)
    --   rot damping
    local _torque = vector.MulNum(_omega, rot_damping * tensor)
    ship.applyInvariantTorque(_torque.x, _torque.y, _torque.z)

    -- fwd
    tmp = vector.MulNum(faces.front, c_fwd * inerita.mass * move_acc)
    ship.applyInvariantForce(tmp.x, tmp.y, tmp.z)

    -- up
    tmp = vector.MulNum(faces.up, c_up * inerita.mass * move_acc)
    ship.applyInvariantForce(tmp.x, tmp.y, tmp.z)

    -- rot ctrl
    --   roll ctrl
    tmp = vector.MulNum(faces.front, -c_roll * tensor * rot_acc)
    ship.applyInvariantTorque(tmp.x, tmp.y, tmp.z)
    --   yaw ctrl
    tmp = vector.MulNum(faces.up, c_yaw * tensor * rot_acc)
    ship.applyInvariantTorque(tmp.x, tmp.y, tmp.z)
    --   pitch ctrl
    tmp = vector.MulNum(faces.right, -c_pitch * tensor * rot_acc)
    ship.applyInvariantTorque(tmp.x, tmp.y, tmp.z)

    -- make vector face to head pointer
    if c_up == 0 then
        up_brake = brake_force*1.5
    else
        up_brake = brake_force*0.25
    end

    tmp = vector.MulNum(vector.Proj(velocity, faces.up), up_brake)
    ship.applyInvariantForce(tmp.x, tmp.y, tmp.z)
    tmp = vector.MulNum(vector.Proj(velocity, faces.right), brake_force*1.5)
    ship.applyInvariantForce(tmp.x, tmp.y, tmp.z)


end

---EventThread
function EventThread()
    ship.setIdle(false)
    --local timer = os.startTimer(0.05)
    while true do
        eventData = { os.pullEvent() }
        if eventData[1] == "phys_tick" then
            OnPhysUpdate(eventData[2])
        end
    end
end

function Safe_HUD_Thread()
    pcall(HUD_Thread)
    ship.setIdle(true)
end

function Safe_Event_Thread()
    pcall(EventThread)
    ship.setIdle(true)
end

function ButtonHandler()

end

function InitController()
    --input_channel:BindButtonCallback("Pressed", ButtonHandler)
end

function Main()
    InitController()
    InitHologram()
    parallel.waitForAll(EventThread, HUD_Thread)
end

Main()
